#include <vector>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_normal_plane.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>

#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/graph/graph_concepts.hpp>

#include <time.h>
// #include <pcl-1.7/pcl/PointIndices.h>

#include "pcl_bridge.h"

#define PI 3.1415926

// 组成平面的最少点数，如果少于这个值，判定已经没有平面了
int minPlanePoints = 50;
// 点云聚类，去除斜坡平面外的杂点，类间点最大距离
double maxClusterdist = 0.13; //m
// 拟合平面时法向量的权重
double NormalDistanceWeight = 0.1;
// 拟合平面时参与计算的点到面的最大距离
double DistanceThreshold = 0.1;//m


bool ransac_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud, double &error, Eigen::VectorXf &coef, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_filtered,
		  pcl::PointCloud<pcl::PointXYZ>::Ptr &ground_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr &stone_cloud){
    // get a plane by ransac
    pcl::PointCloud<pcl::Normal>::Ptr in_normal(new pcl::PointCloud<pcl::Normal>),filtered_normal(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    
    std::cout << "total points num is " << in_cloud->points.size() << std::endl;
    pcl::PassThrough<pcl::PointXYZ> pass; 
    pass.setInputCloud(in_cloud); 
    pass.setFilterFieldName("z"); 
    pass.setFilterLimits(0, 3);
    //保留或过滤z轴方向-1.2到0
    //pass.setFilterLimitsNegative(true);
    //设置过滤器限制负//设置保留范围内false 
    pass.filter(*cloud_filtered); 
    in_cloud.swap(cloud_filtered);
               
    
    // filter
    // Create the filtering object
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud (in_cloud);
    sor.setLeafSize (0.005f, 0.005f, 0.005f);
    sor.filter (*cloud_filtered);

    std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
	<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ").\n";
    std::cout << "ransac the ground plane ...\n";
	
//     std::cout << "going to show the scene cloud,please enter q to go to the next step\n";
//     pcl::visualization::PCLVisualizer viz;
//     viz.addPointCloud<pcl::PointXYZ>(cloud_filtered);
//     viz.spin();
//     cout << "exiting showing the scene cloud\n";

    std::cout << "est norm" << std::endl;
    ne.setInputCloud (cloud_filtered);
    ne.setSearchMethod (tree);
    ne.setRadiusSearch (0.02);
    // Compute the features
    ne.compute (*filtered_normal);
    std::cout << "est norm done" << std::endl;
      
    std::vector<int> ground_inliers;
    pcl::SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal>::Ptr model_p(new pcl::SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal>(cloud_filtered));

    model_p->setInputNormals(filtered_normal);
    model_p->setNormalDistanceWeight (0.5);
    
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);

    ransac.setDistanceThreshold(error);
    ransac.setMaxIterations(100);
    ransac.computeModel(0);
    
    ransac.getModelCoefficients(coef);
    std::cout << "the ground plane coef is \n" << coef << std::endl;
    ransac.getInliers(ground_inliers);
    std::cout << "the ground inliers num is ：" << ground_inliers.size() << std::endl;

    pcl::PointIndices::Ptr inliers_ (new pcl::PointIndices ());
    inliers_->indices = ground_inliers;
    pcl::ExtractIndices<pcl::PointXYZ> extract;    
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers_);
    extract.setNegative (false);
    extract.filter (*ground_cloud);
    
           
//     std::cout << "going to show the ground cloud\n";
//     pcl::visualization::PCLVisualizer viz5;
//     viz5.addPointCloud<pcl::PointXYZ>(ground_cloud);
//     viz5.spin();
//     std::cout << "exciting \n";

    
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_pl(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(in_cloud));
    std::vector<int> inliers;
    model_pl->selectWithinDistance(coef, 0.01, inliers);
    inliers_->indices = inliers;  
    extract.setInputCloud (in_cloud);
    extract.setIndices (inliers_);
    extract.setNegative (true);
    extract.filter (*stone_cloud);
           
//     std::cout << "going to show the stones cloud\n";
//     pcl::visualization::PCLVisualizer viz6;
//     viz6.addPointCloud<pcl::PointXYZ>(stone_cloud);
//     viz6.spin();
//     std::cout << "exciting \n";

    std::cout << "exciting the ransac of the ground\n";
    
    return true;
    
}

void euc_cluster(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud, Eigen::VectorXf &plane, const double euc_threshold, std::vector<pcl::PointCloud<pcl::PointXYZRGB>> &clustered_stones)
{
    // euclidean cluster
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>),cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
   
    std::cout << "going to cluster the stones by eculidean\n";
    
    // filter
    // Create the filtering object
//     pcl::VoxelGrid<pcl::PointXYZ> sor;
//     sor.setInputCloud (in_cloud);
//     sor.setLeafSize (0.005f, 0.005f, 0.005f);
//     sor.filter (*cloud_filtered);
//     in_cloud.swap(cloud_filtered);
// 
//     std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
// 	<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ").\n";
    
    clock_t start, finish;// clock
    start = clock();
    
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    tree->setInputCloud (in_cloud);
    ec.setClusterTolerance (euc_threshold); // 
    ec.setMinClusterSize (100);
    ec.setMaxClusterSize (250000);
    ec.setSearchMethod (tree);
    ec.setInputCloud (in_cloud);
    ec.extract (cluster_indices);
    
    finish = clock();
    double time_used = double(finish - start) / CLOCKS_PER_SEC;
    cout << "pcl cluster time used ---------------------------------------- " << time_used << " s.\n";

	   // visualization
//     boost::shared_ptr<pcl::visualization::PCLVisualizer> euc_viz (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    srand(time(0));
    for(int i = 0; i < cluster_indices.size(); ++i)
    {
	pcl::PointCloud<pcl::PointXYZ>::Ptr stoneXYZ(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud<pcl::PointXYZ>(*in_cloud, cluster_indices[i].indices, *stoneXYZ);
	pcl::PointCloud<pcl::PointXYZRGB> stone;
	pcl::copyPointCloud(*stoneXYZ,stone);
	
	int r = rand()%256;
	int g = rand()%256;
	int b = rand()%256;
	if(stone.points.size()<500) break;
	for(int j = 0; j < stone.points.size(); ++j)
	{
	    stone.points[j].r = r;
	    stone.points[j].g = g;
	    stone.points[j].b = b;
	}
	clustered_stones.push_back(stone);
// 	std::cout << "stone " << i << " has " << stone.size() << " points\n";
// 	euc_viz->removePointCloud();
// 	euc_viz->addPointCloud(stone.makeShared());
// 	euc_viz->spin();
    }
    

    
}




// PlaneAndLineCrossPoint: compute one cross point between plane and line
// [input]: plane   				the plane function coefficients: Ax+By+Cz+D=0; 
//						A = plane(0)
//						B = plane(1)
//						C = plane(2)
//						D = plane(3)
// [input]: pt  				the line function, one point and direction
// [input]: dir  				direction,|dir| = 1
// [output]:out_pt				the result point
void PlaneAndLineCrossPoint(Eigen::VectorXf &plane, Eigen::Vector3d &pt, Eigen::Vector3d &dir, Eigen::Vector3d &out_pt)
{
    Eigen::Vector3d plane_normal(plane(0), plane(1), plane(2));
    double dis = plane_normal.dot(pt)+plane(3);
    out_pt(0) = dir(0)*1000+pt(0);
    out_pt(1) = dir(1)*1000+pt(1);
    out_pt(2) = dir(2)*1000+pt(2);
    double td = plane_normal.dot(out_pt)+plane(3);
    double k = dis / (dis-td);
    out_pt(0) -= pt(0);
    out_pt(1) -= pt(1);
    out_pt(2) -= pt(2);
    out_pt(0) *= k;
    out_pt(1) *= k;
    out_pt(2) *= k;
    out_pt(0) += pt(0);
    out_pt(1) += pt(1);
    out_pt(2) += pt(2);
}

// PlaneAndPlaneCorssLine: compute the cross line between two planes
// [input]: plane1   				the plane function coefficients: Ax+By+Cz+D=0; 
//						A = plane(0)
//						B = plane(1)
//						C = plane(2)
//						D = plane(3)
// [input]: plane2  				the plane function coefficients: Ax+By+Cz+D=0;
// [output]: pt  				the line function, one point and direction
// [output]: dir				direction,|dir| = 1
void PlaneAndPlaneCorssLine(Eigen::VectorXf &plane1, Eigen::VectorXf &plane2, Eigen::Vector3d &pt, Eigen::Vector3d &dir)
{
    Eigen::Vector3d plane_normal1(plane1(0), plane1(1), plane1(2));
    Eigen::Vector3d plane_normal2(plane2(0), plane2(1), plane2(2));
    dir = plane_normal1.cross(plane_normal2);
    double dir_norm = sqrt(dir.dot(dir));
    dir(0) /= dir_norm;
    dir(1) /= dir_norm;
    dir(2) /= dir_norm;
    Eigen::Vector3d pav;
    pav(0) = -plane1(3)*plane1(0);
    pav(1) = -plane1(3)*plane1(1);
    pav(2) = -plane1(3)*plane1(2);
    Eigen::Vector3d tnv;
    tnv = dir.cross(plane_normal1);
    PlaneAndLineCrossPoint(plane2, pav, tnv, pt);
    
}

// SlopeDetection: compute the slope in the input pointcloud
// [input]: cloud   				the input pointcloud
// [output]:plane   				the output slope plane function coefficients: Ax+By+Cz+D=0; 
//						A = plane(0)
//						B = plane(1)
//						C = plane(2)
//						D = plane(3)
// [output]: line_pt  				the line function, one point and direction
// [output]: line_dir				direction,|dir| = 1
// [output]: slope_angle			the detected slope angle in degree (0,90)
// [output]: slope_width			the detected slope width in m
bool SlopeDetection(PointCloud &cloud, Eigen::VectorXf &plane_coef, Eigen::Vector3d &line_pt, Eigen::Vector3d &line_dir, Eigen::Vector3d &max_point, Eigen::Vector3d &min_point, double &slope_angle, double &slope_width)
{
    // detection slope iterately and output the relative initCameraParameters
    pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZ>),cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCDWriter writer;
    in_cloud->width = cloud.size();
    in_cloud->height = 1;
    in_cloud->resize(in_cloud->width*in_cloud->height);
    
    std::cout << "total points num is " << cloud.size() << std::endl;
    
    for(size_t i = 0; i < cloud.size(); ++i)
    {
	in_cloud->points[i].x = cloud[i].x;
	in_cloud->points[i].y = cloud[i].y;
	in_cloud->points[i].z = cloud[i].z;
    }
    
    // 采样一致性模型对象
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
    pcl::PointCloud<pcl::Normal>::Ptr in_normal(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    // Create the filtering object
    pcl::ExtractIndices<pcl::PointXYZ> extract;
//     int minPlanePoints = 50;
//     double maxClusterdist = 0.13; //7cm
    
    int i = 0;
    Eigen::VectorXf ground_coef = Eigen::VectorXf::Zero(4 , 1);
    bool exist_slope = false;
    
    while(true)
    {
	// extract plane
	std::vector<int> minliers;

	ne.setInputCloud (in_cloud);
	ne.setSearchMethod (tree);
	ne.setRadiusSearch (1);
	// Compute the features
	ne.compute (*in_normal);
	
	pcl::SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal>::Ptr model_p(new pcl::SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal>(in_cloud));
	model_p->setInputNormals(in_normal);
	model_p->setNormalDistanceWeight (NormalDistanceWeight);
	
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);

	ransac.setDistanceThreshold(DistanceThreshold);
	ransac.computeModel(1);
	ransac.getInliers(minliers);

	std::cout << "局内点：" << minliers.size() << std::endl;
	
	if (minliers.size() < minPlanePoints)
	{
	    cout << "no plane to extract anymore,exiting...\n";
	    break;
	}
	
	Eigen::VectorXf coef = Eigen::VectorXf::Zero(4 , 1);
	ransac.getModelCoefficients(coef);
	std::cout << "the plane coef is \n" << coef << std::endl;
	if ( i == 0)  
	{
	    ground_coef = coef;
	    std::cout << "the ground coef is \n" << ground_coef << std::endl;
	    
	}
	
	pcl::visualization::PCLVisualizer viz;
	viz.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(in_cloud, in_normal, 1, 0.2);
	viz.spin();
	
	inliers->indices = minliers;
	
	// Extract the inliers
	extract.setInputCloud (in_cloud);
	extract.setIndices (inliers);
	extract.setNegative (false);
	extract.filter (*cloud_p);
	std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

	std::stringstream ss;
	ss << "plane" << i << ".pcd";
	writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
	
	// stop or not
	////////////////////////////
	if ( i > 0)
	{
	    // compute the slope angle
	    slope_angle = acos(ground_coef(0)*coef(0)+ground_coef(1)*coef(1)+ground_coef(2)*coef(2));
	    slope_angle = slope_angle/PI*180;
// 	    cout << "slope angle is " << slope_angle << endl;
	    if (slope_angle > 90) slope_angle = 180-slope_angle;
	    if (slope_angle > 9 && slope_angle < 61)
	    {
		exist_slope = true;
		cout << "found a slope with angle " << slope_angle << endl;
		// get the other Parameters
		plane_coef = coef;
		
		tree->setInputCloud (cloud_p);

		std::vector<pcl::PointIndices> cluster_indices;
		pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
		ec.setClusterTolerance (maxClusterdist); // 
		ec.setMinClusterSize (10);
		ec.setMaxClusterSize (25000);
		ec.setSearchMethod (tree);
		ec.setInputCloud (cloud_p);
		ec.extract (cluster_indices);

		int j = 0;
		int max_cluster_points = 0;
		int max_cluster_id;
		
		cout << "the cluster num is "<< cluster_indices.size() << endl;
		if (cluster_indices.size() > 0)
		{
		    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
		    {
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
			for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
			    cloud_cluster->points.push_back(cloud_p->points[*pit]); //*
			cloud_cluster->width = cloud_cluster->points.size();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;
			if (cloud_cluster->width > max_cluster_points){
			    max_cluster_points = cloud_cluster->width;
			    max_cluster_id = j;
			}
			std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
			std::stringstream ss;
			ss << "cloud_cluster_" << j << ".pcd";
			writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
			j++;
		    }
		    // the slope plane id is max_cluster_id
		    
		    pcl::PointCloud<pcl::PointXYZ>::Ptr slope(new pcl::PointCloud<pcl::PointXYZ>);
		    pcl::copyPointCloud<pcl::PointXYZ>(*cloud_p, cluster_indices[max_cluster_id].indices, *slope);
		    
		    // compute the slope plane again with the extracted slope pointcloud
		    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_slope(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(slope));
		    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac_slope(model_slope);
		    ransac_slope.setDistanceThreshold(0.02);
		    ransac_slope.computeModel();
		    ransac_slope.getModelCoefficients(plane_coef);
		    std::vector<int> slope_inliers;
		    ransac_slope.getInliers(slope_inliers);
		    cout << "total slope points " << slope->points.size() << endl;
		    cout << "slope inliers size " << slope_inliers.size() << endl;
		    
		    
		    // compute the cross line between the slope plane and the ground
		    PlaneAndPlaneCorssLine(ground_coef, plane_coef, line_pt, line_dir);

		    //compute length of the line
		    std::vector<float> dist(slope->size());
		    for (int k = 0; k<slope->size(); ++k)
		    {
			Eigen::Vector3d diff;
			diff(0) = slope->points[k].x-line_pt(0);
			diff(1) = slope->points[k].y-line_pt(1);
			diff(2) = slope->points[k].z-line_pt(2);
			Eigen::Vector3d dir(line_dir);
			dist[k] = diff.dot(dir);
		    }
		    std::vector<float>::iterator max_iter = std::max_element(dist.begin(), dist.end());
		    std::vector<float>::iterator min_iter = std::min_element(dist.begin(), dist.end());
		    slope_width = *max_iter - *min_iter;
// 		    Eigen::Vector3d max_point,min_point;
		    pcl::PointXYZ max_p,min_p;
		    for (int i = 0; i < 3; ++i)
			max_point(i) = line_dir(i)*(*max_iter)+line_pt(i);
		    for (int i = 0; i < 3; ++i)
			min_point(i) = line_dir(i)*(*min_iter)+line_pt(i);
		    max_p.x = max_point(0);
		    max_p.y = max_point(1);
		    max_p.z = max_point(2);
		    min_p.x = min_point(0);
		    min_p.y = min_point(1);
		    min_p.z = min_point(2);
		    pcl::visualization::PCLVisualizer viz1;
		    viz1.addPointCloud<pcl::PointXYZ>(slope);
		    pcl::ModelCoefficients coeffs;
		    coeffs.values.push_back(ground_coef(0));
		    coeffs.values.push_back(ground_coef(1));
		    coeffs.values.push_back(ground_coef(2));
		    coeffs.values.push_back(ground_coef(3));
		    viz1.addPlane(coeffs,"ground");
		    pcl::ModelCoefficients slopecoeffs;
		    slopecoeffs.values.push_back(plane_coef(0));
		    slopecoeffs.values.push_back(plane_coef(1));
		    slopecoeffs.values.push_back(plane_coef(2));
		    slopecoeffs.values.push_back(plane_coef(3));
		    viz1.addPlane(slopecoeffs,"slope");
		    viz1.addLine<pcl::PointXYZ>(min_p, max_p);
		    viz1.spin();
		}
		
		break;
	    }
	}

	// Create the filtering object
	extract.setNegative (true);
	extract.filter(*cloud_f);
	in_cloud.swap(cloud_f);
	i++;
    }
    
    return exist_slope;
    
}

// SlopeDetection: compute the slope in the input pointcloud
// [input]: cloud   				the input pointcloud
// [output]:plane   				the output slope plane function coefficients: Ax+By+Cz+D=0; 
//						A = plane(0)
//						B = plane(1)
//						C = plane(2)
//						D = plane(3)
// [output]: line_pt  				the line function, one point and direction
// [output]: line_dir				direction,|dir| = 1
// [output]: slope_angle			the detected slope angle in degree (0,90)
// [output]: slope_width			the detected slope width in m
bool SlopeDetectionwithAll(PointCloud &all_cloud, PointCloud &front_cloud, Eigen::VectorXf &plane_coef, Eigen::Vector3d &line_pt, Eigen::Vector3d &line_dir, Eigen::Vector3d &max_point, Eigen::Vector3d &min_point, double &slope_angle, double &slope_width)
{
    // detection slope iterately and output the relative initCameraParameters
    pcl::PointCloud<pcl::PointXYZ>::Ptr in_all_cloud(new pcl::PointCloud<pcl::PointXYZ>), in_cloud(new pcl::PointCloud<pcl::PointXYZ>),cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCDWriter writer;
    in_all_cloud->width = all_cloud.size();
    in_all_cloud->height = 1;
    in_all_cloud->resize(in_all_cloud->width*in_all_cloud->height);
    
    in_cloud->width = front_cloud.size();
    in_cloud->height = 1;
    in_cloud->resize(in_cloud->width*in_cloud->height);
    
    std::cout << "total points num is " << all_cloud.size() << std::endl;
    
    for(size_t i = 0; i < all_cloud.size(); ++i)
    {
	in_all_cloud->points[i].x = all_cloud[i].x;
	in_all_cloud->points[i].y = all_cloud[i].y;
	in_all_cloud->points[i].z = all_cloud[i].z;
    }
    
        for(size_t i = 0; i < front_cloud.size(); ++i)
    {
	in_cloud->points[i].x = front_cloud[i].x;
	in_cloud->points[i].y = front_cloud[i].y;
	in_cloud->points[i].z = front_cloud[i].z;
    }
    
    
    // 采样一致性模型对象
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
    pcl::PointCloud<pcl::Normal>::Ptr in_normal(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    // Create the filtering object
    pcl::ExtractIndices<pcl::PointXYZ> extract;
//     int minPlanePoints = 50;
//     double maxClusterdist = 0.13; //7cm
    
    int i = 0;
    Eigen::VectorXf ground_coef = Eigen::VectorXf::Zero(4 , 1);
    bool exist_slope = false;
    
    // extract ground with all pointcloud first
    ne.setInputCloud (in_all_cloud);
    ne.setSearchMethod (tree);
    ne.setRadiusSearch (1);
    // Compute the features
    ne.compute (*in_normal);
    
    pcl::SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal>::Ptr model_ground(new pcl::SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal>(in_all_cloud));
    model_ground->setInputNormals(in_normal);
    model_ground->setNormalDistanceWeight (NormalDistanceWeight);
    
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac_ground(model_ground);

    ransac_ground.setDistanceThreshold(DistanceThreshold);
    ransac_ground.computeModel(1);
    ransac_ground.getModelCoefficients(ground_coef);
    std::cout << "the ground coef is \n" << ground_coef << std::endl;
    
    while(true)
    {
	// extract plane
	std::vector<int> minliers;

	ne.setInputCloud (in_cloud);
	ne.setSearchMethod (tree);
	ne.setRadiusSearch (1);
	// Compute the features
	ne.compute (*in_normal);
	
	pcl::SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal>::Ptr model_p(new pcl::SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal>(in_cloud));
	model_p->setInputNormals(in_normal);
	model_p->setNormalDistanceWeight (NormalDistanceWeight);
	
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);

	ransac.setDistanceThreshold(DistanceThreshold);
	ransac.computeModel(1);
	ransac.getInliers(minliers);

	std::cout << "局内点：" << minliers.size() << std::endl;
	
	if (minliers.size() < minPlanePoints)
	{
	    cout << "no plane to extract anymore,exiting...\n";
	    break;
	}
	
	Eigen::VectorXf coef = Eigen::VectorXf::Zero(4 , 1);
	ransac.getModelCoefficients(coef);
	std::cout << "the plane coef is \n" << coef << std::endl;
	
	pcl::visualization::PCLVisualizer viz;
	viz.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(in_cloud, in_normal, 1, 0.2);
	viz.spin();
	
	inliers->indices = minliers;
	
	// Extract the inliers
	extract.setInputCloud (in_cloud);
	extract.setIndices (inliers);
	extract.setNegative (false);
	extract.filter (*cloud_p);
	std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

	std::stringstream ss;
	ss << "plane" << i << ".pcd";
	writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
	
	// stop or not
	////////////////////////////
	if ( i > 0)
	{
	    // compute the slope angle
	    slope_angle = acos(ground_coef(0)*coef(0)+ground_coef(1)*coef(1)+ground_coef(2)*coef(2));
	    slope_angle = slope_angle/PI*180;
	    if (slope_angle > 90) slope_angle = 180 - slope_angle;
// 	    cout << "slope angle is " << slope_angle << endl;
	    if (slope_angle > 9 && slope_angle < 61)
	    {
		exist_slope = true;
		cout << "found a slope with angle " << slope_angle << endl;
		// get the other Parameters
		plane_coef = coef;
		
		tree->setInputCloud (cloud_p);

		std::vector<pcl::PointIndices> cluster_indices;
		pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
		ec.setClusterTolerance (maxClusterdist); // 2cm
		ec.setMinClusterSize (10);
		ec.setMaxClusterSize (25000);
		ec.setSearchMethod (tree);
		ec.setInputCloud (cloud_p);
		ec.extract (cluster_indices);

		int j = 0;
		int max_cluster_points = 0;
		int max_cluster_id;
		
		cout << "the cluster num is "<< cluster_indices.size() << endl;
		if (cluster_indices.size() > 0)
		{
		    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
		    {
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
			for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
			    cloud_cluster->points.push_back(cloud_p->points[*pit]); //*
			cloud_cluster->width = cloud_cluster->points.size();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;
			if (cloud_cluster->width > max_cluster_points){
			    max_cluster_points = cloud_cluster->width;
			    max_cluster_id = j;
			}
			std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
			std::stringstream ss;
			ss << "cloud_cluster_" << j << ".pcd";
			writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
			j++;
		    }
		    // the slope plane id is max_cluster_id
		    
		    pcl::PointCloud<pcl::PointXYZ>::Ptr slope(new pcl::PointCloud<pcl::PointXYZ>);
		    pcl::copyPointCloud<pcl::PointXYZ>(*cloud_p, cluster_indices[max_cluster_id].indices, *slope);
		    
		    // compute the slope plane again with the extracted slope pointcloud
		    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_slope(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(slope));
		    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac_slope(model_slope);
		    ransac_slope.setDistanceThreshold(0.02);
		    ransac_slope.computeModel();
		    ransac_slope.getModelCoefficients(plane_coef);
		    std::vector<int> slope_inliers;
		    ransac_slope.getInliers(slope_inliers);
		    cout << "total slope points " << slope->points.size() << endl;
		    cout << "slope inliers size " << slope_inliers.size() << endl;
		    
		    // compute the cross line between the slope plane and the ground
		    PlaneAndPlaneCorssLine(ground_coef, plane_coef, line_pt, line_dir);

		    //compute length of the line
		    std::vector<float> dist(slope->size());
		    for (int k = 0; k<slope->size(); ++k)
		    {
			Eigen::Vector3d diff;
			diff(0) = slope->points[k].x-line_pt(0);
			diff(1) = slope->points[k].y-line_pt(1);
			diff(2) = slope->points[k].z-line_pt(2);
			Eigen::Vector3d dir(line_dir);
			dist[k] = diff.dot(dir);
		    }
		    std::vector<float>::iterator max_iter = std::max_element(dist.begin(), dist.end());
		    std::vector<float>::iterator min_iter = std::min_element(dist.begin(), dist.end());
		    slope_width = *max_iter - *min_iter;
// 		    Eigen::Vector3d max_point,min_point;
		    pcl::PointXYZ max_p,min_p;
		    for (int i = 0; i < 3; ++i)
			max_point(i) = line_dir(i)*(*max_iter)+line_pt(i);
		    for (int i = 0; i < 3; ++i)
			min_point(i) = line_dir(i)*(*min_iter)+line_pt(i);
		    max_p.x = max_point(0);
		    max_p.y = max_point(1);
		    max_p.z = max_point(2);
		    min_p.x = min_point(0);
		    min_p.y = min_point(1);
		    min_p.z = min_point(2);
		    pcl::visualization::PCLVisualizer viz1;
		    viz1.addPointCloud<pcl::PointXYZ>(cloud_p);
		    pcl::ModelCoefficients coeffs;
		    coeffs.values.push_back(ground_coef(0));
		    coeffs.values.push_back(ground_coef(1));
		    coeffs.values.push_back(ground_coef(2));
		    coeffs.values.push_back(ground_coef(3));
		    viz1.addPlane(coeffs,"ground");
		    pcl::ModelCoefficients slopecoeffs;
		    slopecoeffs.values.push_back(plane_coef(0));
		    slopecoeffs.values.push_back(plane_coef(1));
		    slopecoeffs.values.push_back(plane_coef(2));
		    slopecoeffs.values.push_back(plane_coef(3));
		    viz1.addPlane(slopecoeffs,"slope");
		    viz1.addLine<pcl::PointXYZ>(min_p, max_p);
		    viz1.spin();
		}
		
		break;
	    }
	}

	// Create the filtering object
	extract.setNegative (true);
	extract.filter(*cloud_f);
	in_cloud.swap(cloud_f);
	i++;
    }
    
    return exist_slope;
    
}
